#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright (C) 2023 reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/4/8 下午4:11
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : test3.py
# @Software: PyCharm

import cv2 as cv
import numpy as np
from arm_control import uarm_control
from vision import transform, detection
import matplotlib.pyplot as plt

mtx = np.load("../data/camera_param.npz")['mtx']
with np.load('../data/eyehand_Matrix.npz') as X:
    R, t = [X[i] for i in ('R', 't')]
arm = uarm_control.Arm_controller(mtx,R,t)
arm.arm_init()

tf = transform.Transform(mtx)

cap = cv.VideoCapture(4)
while cap.isOpened():
    _,frame = cap.read()

    cv.imshow("image",frame)
    if cv.waitKey(10) == 27:
        break
cv.destroyAllWindows()
cap.release()

arm.tag_point(frame,8,20)